﻿using System;
using System.Collections.Generic;

namespace Pathfinding
{
    public class Heap<T> where T : IComparable<T>
    {
        public int NowLength { get; private set; }
        public int MaxLength { get; private set; }
        public T Top => _heap[0];
        public bool IsEmpty => NowLength == 0;
        public bool IsFull => NowLength >= MaxLength - 1;
        private readonly Dictionary<T, int> _nodeIdxTable; // 记录结点在数组中的位置，方便查找
        private readonly bool _isReverse;
        private readonly T[] _heap;

        public Heap(int maxLength, bool isReverse = false)
        {
            NowLength = 0;
            MaxLength = maxLength;
            _heap = new T[MaxLength + 1];
            _nodeIdxTable = new Dictionary<T, int>();
            this._isReverse = isReverse;
        }

        public T this[int index] => _heap[index];

        public void PushHeap(T value)
        {
            if (NowLength >= MaxLength) return;
            _nodeIdxTable[value] = NowLength;
            _heap[NowLength] = value;
            Swim(NowLength);
            ++NowLength;
        }

        public void PopHeap()
        {
            if (NowLength <= 0) return;
            _nodeIdxTable[_heap[0]] = -1;
            _heap[0] = _heap[--NowLength];
            _nodeIdxTable[_heap[0]] = 0;
            Sink(0);
        }

        public bool Contains(T value)
        {
            return _nodeIdxTable.ContainsKey(value) && _nodeIdxTable[value] != -1;
        }

        public T Find(T value)
        {
            return Contains(value) ? _heap[_nodeIdxTable[value]] : default;
        }

        public void Clear()
        {
            _nodeIdxTable.Clear();
            NowLength = 0;
        }

        private void SwapValue(T a, T b)
        {
            var aIdx = _nodeIdxTable[a];
            var bIdx = _nodeIdxTable[b];
            _heap[aIdx] = b;
            _heap[bIdx] = a;
            _nodeIdxTable[a] = bIdx;
            _nodeIdxTable[b] = aIdx;
        }

        private void Swim(int index)
        {
            while (index > 0)
            {
                var father = (index - 1) >> 1;
                if (IsBetter(_heap[index], _heap[father]))
                {
                    SwapValue(_heap[father], _heap[index]);
                    index = father;
                }
                else return;
            }
        }

        private void Sink(int index)
        {
            var left = (index << 1) + 1;
            while (left < NowLength)
            {
                var largest = left + 1 < NowLength && IsBetter(_heap[left + 1], _heap[left]) ? left + 1 : left;
                if (IsBetter(_heap[index], _heap[largest]))
                    largest = index;
                if (largest == index) return;
                SwapValue(_heap[largest], _heap[index]);
                index = largest;
                left = (index << 1) + 1;
            }
        }

        private bool IsBetter(T v1, T v2)
        {
            return _isReverse ? (v2.CompareTo(v1) < 0) : (v1.CompareTo(v2) < 0);
        }
    }
}